﻿using System.Collections.Generic;
using LFSRecord2.Misc;
using InSimDotNet.Packets;
using System.Diagnostics;
using System.Collections.ObjectModel;
using LFSRecord2.Model.Replay;

namespace LFSRecord2.Model
{
    public static class CustomDoubleInterpolation
    {
        public static double InterpolateValue(CustomDoubleKeyFrame fromKey, CustomDoubleKeyFrame toKey, int frame)
        {
            if (fromKey.FrameNum == frame)
                return fromKey.Value;

            // Normalize the time values
            frame -= fromKey.FrameNum;
            int numFrames = toKey.FrameNum - fromKey.FrameNum;

            if (fromKey.InterpolationTypeOut == KeyFrameInterpolationTypes.AddElapsed)
            {
                return fromKey.Value + frame * 1.0d / LfsRecordController.project.FPS;
            }
            else
            {
                // Easing
                double normalizedTime = CustomDoubleEase.EasingCore((double)frame / (double)numFrames, fromKey.EaseOutValue, toKey.EaseInValue);
                return InterpolateLinear(fromKey.Value, toKey.Value, normalizedTime);
            }
        }

        public static double InterpolateLinearOrientationShortestRoute(double fromValue, double toValue, double normalizedTime)
        {
            double diffX = toValue - fromValue;
            if (diffX < 0)
            {
                if (diffX > -180)
                    return InterpolateLinear(fromValue, toValue, normalizedTime);
                else
                    return InterpolateLinear(fromValue, toValue + 360, normalizedTime);
            }
            else
            {
                if (diffX < 180)
                    return InterpolateLinear(fromValue, toValue, normalizedTime);
                else
                    return InterpolateLinear(fromValue, toValue - 360, normalizedTime);
            }

        }

        public static double InterpolateLinear(double fromValue, double toValue, double normalizedTime)
        {
            return (normalizedTime * (toValue - fromValue) + fromValue);
        }

        public static double InterpolateMciValue(Collection<CompCar> mciPackets, string mciValue, double replayTime)
        {
            double mciTime = 0.05d;
            double startValue = GetMciValue(mciPackets, mciValue, 0);
            double endValue = -1;

            for (int a = 1; a < mciPackets.Count; a++)
            {
                if (replayTime < mciTime)
                {
                    endValue = GetMciValue(mciPackets, mciValue, a);
                    break;
                }
                startValue = GetMciValue(mciPackets, mciValue, a);

                mciTime += 0.05d;
            }
            if (endValue == -1)
                return startValue;

            // Normalize the time
            replayTime -= (mciTime - 0.05d);

            // Interpolate between the start and end value
            return InterpolateLinear(startValue, endValue, replayTime / 0.05d);
        }

        public static double InterpolateMciHeading(Collection<CompCar> mciPackets, double replayTime)
        {
            double mciTime = 0.05d;
            double heading = mciPackets[0].Heading;
            double headingPrevious = 0;

            int numHeadingRevolves = 0;
            int numHeadingRevolvesPrevious = 0;

            // Interpolate heading value, using heading and angvel
            for (int a = 1; a < mciPackets.Count; a++)
            {
                headingPrevious = heading;
                heading = mciPackets[a].Heading;
                numHeadingRevolvesPrevious = numHeadingRevolves;

                // Check if this rotation is ending up below 0 or above 360
                if (heading - 31678 > headingPrevious)
                {
                    numHeadingRevolves--;
                }
                else if (heading + 31678 < headingPrevious)
                {
                    numHeadingRevolves++;
                }

                if (replayTime < mciTime)
                {
                    break;
                }

                mciTime += 0.05d;
            }

            replayTime -= (mciTime - 0.05d);

            //Debug.WriteLine(
            //    "Old numheading {0}, angvel {1} ({2}) - new numheading {3}, angvel {4} ({5})",
            //    numHeadingRevolvesPrevious,
            //    angVelPrevious / LfsHelper.LfsDegMult,
            //    headingPrevious / LfsHelper.LfsDegMult,
            //    numHeadingRevolves,
            //    angVel / LfsHelper.LfsDegMult,
            //    heading / LfsHelper.LfsDegMult);
            return InterpolateLinear(
                headingPrevious + numHeadingRevolvesPrevious * 360 * LfsHelper.LfsDegMult, 
                heading + numHeadingRevolves * 360 * LfsHelper.LfsDegMult, 
                replayTime / 0.05d);
        }

        private static double GetMciValue(Collection<CompCar> mciPackets, string mciValue, int index)
        {
            switch (mciValue)
            {
                case "X":
                    return mciPackets[index].X;
                case "Y":
                    return mciPackets[index].Y;
                case "Z":
                    return mciPackets[index].Z;
                case "Heading":
                    return mciPackets[index].Heading;
            }
            return 0;
        }
    }
}
